#ifndef __MAP_H
#define __MAP_H
#include <vector>
#include <string>
#include "RTree.h"
#include "PolygonalMesh.h"
#include "MapPoint.h"
#include "KeyFrame.h"
#include <TooN/se3.h>
#include <cvd/image.h>
#include <boost/thread/shared_mutex.hpp>
#include <boost/shared_ptr.hpp>
#include <set>

struct MapPoint;

class Map
{
public:
  Map();
  ~Map();
  inline bool IsGood() { return bGood; }
  void SetGood() { bGood = true; }
  void SetBad() { bGood = false; }
  void Reset();
  void Save();
  const SE3GaussianPDF& GetGfromF() { return mpFirstKeyFrame->mse3PdfGfromC; };
  const SE3<>& GetFfromW() { return mpFirstKeyFrame->se3CfromW; };
  const SE3<>* GetPtrFfromW() { return &(mpFirstKeyFrame->se3CfromW); };

  // Memory to store keyframes
  boost::shared_mutex mKeyFramesLock;
  RTree<KeyFrame, double, 3> mrtAllKeyFrames;
  std::set<boost::shared_ptr<KeyFrame> > msLocalOptimKeyFrames;
  std::set<boost::shared_ptr<KeyFrame> > msLocalFixedKeyFrames;

  // Memory to store mappoints
  boost::shared_mutex mMapPointsLock;
  std::vector<boost::shared_ptr<MapPoint> > mvAllMapPoints;
  std::set<boost::shared_ptr<MapPoint> > msLocalMapPoints;

  // Memory to store 3d textured mesh
  boost::shared_mutex mMeshLock;
  PolygonalMesh mesh;

  std::vector<std::pair<std::string, TooN::SE3<> > > vSnapshotPoses;

  bool bGood;

  boost::shared_ptr<KeyFrame> mpFirstKeyFrame;
};

#endif

#include <boost/interprocess/detail/config_end.hpp>
